# Copyright (c) Phigent Robotics. All rights reserved.

_base_ = ['../_base_/datasets/nus-3d.py',
            # '../_base_/models/centerpoint_02pillar_second_secfpn_nus.py',
            '../_base_/default_runtime.py']
# Global
# If point cloud range is changed, the models should also change their point
# cloud range accordingly
point_cloud_range = [-51.2, -51.2, -5.0, 51.2, 51.2, 3.0]
# For nuScenes we usually do 10-class detection
class_names = [
    'car', 'truck', 'construction_vehicle', 'bus', 'trailer', 'barrier',
    'motorcycle', 'bicycle', 'pedestrian', 'traffic_cone'
]

data_config = {
    'cams': [
        'front', 'right_front', 'right_back', 'back','left_back','left_front'
    ],
    'Ncams':
    6,
    'input_size': (256, 704),
    'src_size': (900, 1600),

    # Augmentation
    "resize": dict(front=(-0.05,0.4),
            right_front=(-0.05,0.2),
            right_back=(-0.1, 0.1),
            back=(-0.01,0.4),
            left_back=(0.2,0.2),
            left_front=(-0.05,0.2),),
    "crop_h": dict(front=(0.0,0.15),
            right_front=(-0.04, 0.3),
            right_back=(-0.1, 0.2),
            back=(0.0,0.5),
            left_back=(-0.1,0.2),
            left_front=(-0.05,0.2),),
    "resize_test": dict(front=0.2,
            right_front=0.0,
            right_back=0.0,
            back=0.1,
            left_back=0.0,
            left_front=0.0,),
    # 'resize': (-0.06, 0.11),
    'rot': (-5.4, 5.4),
    'flip': True,
    # 'crop_h': (0.0, 0.0),
    # 'resize_test': 0.00,
    'pixel_aug': 0.3,
}

# Model
grid_config = {
    'x': [-51.2, 51.2, 0.8],
    'y': [-51.2, 51.2, 0.8],
    'z': [-5, 3, 8],
    'depth': [1.0, 60.0, 0.5], # 0.5
}

voxel_size = [0.2, 0.2, 8]


numC_Trans = 64
numC_bev = 64
sequ_num = 10
sequ_mode = True # for infer param
lidar="vehicle" # vehicle inno_frl_0_raw inno_frl_1_raw
model = dict(
    type='BEVDet4d_Fusion',
    Nframe=False,
    sequ_num=sequ_num,
    with_img=True,
    with_pts=False,
    bev_align=True,
    sequ_mode=sequ_mode,
    img_backbone=dict(
        pretrained='torchvision://resnet50',
        type='ResNet',
        depth=50,
        num_stages=4,
        out_indices=(2, 3),
        frozen_stages=-1,
        norm_cfg=dict(type='BN', requires_grad=True),
        norm_eval=False,
        with_cp=True,
        style='pytorch'),
    img_neck=dict(
        type='CustomFPN',
        in_channels=[1024, 2048],
        out_channels=512,
        num_outs=1,
        start_level=0,
        out_ids=[0]),
    img_view_transformer=dict(
        type='LSSViewTransformerMatrixVT',
        grid_config=grid_config,
        input_size=data_config['input_size'],
        in_channels=512,
        out_channels=numC_Trans,
        # depthnet_cfg=dict(use_dcn=False, aspp_mid_channels=96),
        downsample=16,
        accumuate_map=2, # mode 1: acc 0, norm True; mode 2: acc 1 , norm True; mode 3: acc 2, norm False
        norm_map=False),
    pre_process_img=dict(
        type='CustomResNet',
        numC_input=numC_Trans,
        num_layer=[2,],
        num_channels=[numC_bev,],
        stride=[1,],),

    img_bev_encoder_backbone=dict(
        type='CustomResNet',
        numC_input=numC_bev,
        num_channels=[numC_bev * 2, numC_bev * 4, numC_bev * 8]),
    img_bev_encoder_neck=dict(
        type='FPN_LSS',
        in_channels=numC_bev * 8 + numC_bev * 2,
        out_channels=256),
    # points
    # pts_voxel_layer=dict(
    #     max_num_points=9, 
    #     voxel_size=voxel_size, 
    #     max_voxels=(30000, 40000),
    #     point_cloud_range=point_cloud_range),
    # pts_voxel_encoder=dict(
    #     type='PillarFeatureNet',
    #     in_channels=4,
    #     feat_channels=[64],
    #     with_distance=False,                                                                                              
    #     with_cluster_center=False,
    #     with_voxel_center=False,
    #     only_voxel_center=True,
    #     voxel_size=(0.2, 0.2, 8),
    #     norm_cfg=None,
    #     legacy=False,
    #     point_cloud_range=point_cloud_range,
    #     ),
    # pts_middle_encoder=dict(
    #     type='PointPillarsScatter', in_channels=64, output_shape=(512, 512)),
    # pts_backbone=dict(
    #     type='SECOND',
    #     in_channels=64,
    #     out_channels=[64, 128, 256],
    #     layer_nums=[3, 5, 5],
    #     layer_strides=[2, 2, 2],
    #     norm_cfg=dict(type='BN', eps=1e-3, momentum=0.01),
    #     conv_cfg=dict(type='Conv2d', bias=False)),
    # pts_neck=dict(
    #     type='SECONDFPN',
    #     in_channels=[64, 128, 256],
    #     out_channels=[128, 128, 128],
    #     upsample_strides=[0.5, 1, 2],
    #     norm_cfg=dict(type='BN', eps=1e-3, momentum=0.01),
    #     upsample_cfg=dict(type='deconv', bias=False),
    #     use_conv_for_no_stride=True),

    # fusion
    # feat_fusion_layer=dict(
    #     type='FeatFusion',
    #     fusion_type="Fusion_block1",
    #     in_channels=[numC_bev, numC_bev],
    #     out_channels=numC_bev,
    #     norm_cfg=dict(type='BN', eps=1e-3, momentum=0.01),
    # ),
    # pre_process_pts=dict(
    #     type='CustomResNet',
    #     numC_input=sum([128,128,128]),
    #     num_layer=[2,],
    #     num_channels=[numC_bev,],
    #     stride=[1,],),
    # pre_process_fusion=dict(
    #     type='CustomResNet',
    #     numC_input=numC_bev * 2,
    #     num_layer=[2,],
    #     num_channels=[numC_bev * 2,],
    #     stride=[1,],),
    # 4d fusion
    feat4d_fusion_layer=dict(
        type='FeatFusion',
        fusion_type="GRU_CNN",
        in_channels=[numC_bev, numC_bev],
        out_channels=numC_bev,
        norm_cfg=dict(type='BN', eps=1e-3, momentum=0.01),
        align_mode='bilinear',
        align_method=1,
        align_corners=False,
        detach = True,
        Rpose=False,
        encode_t=True,
        encode_t_num=8,
        aug_p=0.3,    
    ),
    # after_process_fusion=dict(
    #     type='CustomResNet',
    #     numC_input=numC_bev,
    #     num_layer=[2,],
    #     num_channels=[numC_bev,],
    #     stride=[1,],),
    # pts_bbox_head=None,
    pts_bbox_head=dict(
        type='CenterHead',
        in_channels=256,
        tasks=[
            dict(num_class=10, class_names=['car', 'truck',
                                            'construction_vehicle',
                                            'bus', 'trailer',
                                            'barrier',
                                            'motorcycle', 'bicycle',
                                            'pedestrian', 'traffic_cone']),
        ],
        common_heads=dict(
            reg=(2, 2), height=(1, 2), dim=(3, 2), rot=(2, 2), vel=(2, 2)),
        share_conv_channel=64,
        bbox_coder=dict(
            type='CenterPointBBoxCoder',
            pc_range=point_cloud_range[:2],
            post_center_range=[-61.2, -61.2, -10.0, 61.2, 61.2, 10.0],
            max_num=500,
            score_threshold=0.1,
            out_size_factor=4,
            voxel_size=voxel_size[:2],
            code_size=9),
        separate_head=dict(
            type='SeparateHead', init_bias=-2.19, final_kernel=3),
        loss_cls=dict(type='GaussianFocalLoss', reduction='mean',loss_weight=6.),
        loss_bbox=dict(type='L1Loss', reduction='mean', loss_weight=1.5),
        norm_bbox=True),
    # 3d maps detection
    map_head=dict(
        type='BevMapHead',
        in_channels=256,
        # 'crosswalk','stop_line','speed_bump','no_stop','turn'
        tasks=[
            dict(num_class=3, class_names=['road_edge']),
            dict(num_class=5, class_names=['lane']),
            dict(num_class=3, class_names=['center_lines']),
            dict(num_class=3, class_names=['crosswalk']),
            dict(num_class=3, class_names=['stop_line']),
            dict(num_class=3, class_names=['speed_bump']),
            dict(num_class=3, class_names=['no_stop']),
            dict(num_class=8, class_names=['turn']),
        ],
        common_heads=dict(
            reg=(4, 2)),
        share_conv_channel=64,
        map_coder=dict(
            type="CenterPointBBoxCoder", # 'BevmapCoder',
            pc_range=point_cloud_range[:2],
            post_center_range=[-61.2, -61.2, -10.0, 61.2, 61.2, 10.0],
            max_num=500,
            score_threshold=0.1,
            out_size_factor=4,
            voxel_size=voxel_size[:2],
            code_size=9),
        separate_head=dict(
            type='SeparateHead', init_bias=-2.19, final_kernel=3),
        loss_cls=dict(type='GaussianFocalLoss', reduction='mean'),
        loss_map=dict(type='L1Loss', reduction='mean', loss_weight=1.0),
        norm_map=True),
    # model training and testing settings
    train_cfg=dict(
        pts=dict(
            point_cloud_range=point_cloud_range,
            grid_size=[512, 512, 40],
            voxel_size=voxel_size,
            out_size_factor=4,
            dense_reg=1,
            gaussian_overlap=0.1,
            max_objs=500,
            min_radius=2,
            code_weights=[1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0],
            loss_weight=1.0,
            ),
        maps=dict(
            point_cloud_range=point_cloud_range,
            grid_size=[512, 512, 40],
            voxel_size=voxel_size,
            out_size_factor=4,
            preds_per_voxel=2,
            code_weights=[2.0, 2.0],
            cls_weights=2.0,
            markv_weights=0.5,
            aux_weights = 2.0,
            loss_weight=0.5,
            reg_std_weights=3.0,
            max_objs=1e4,
            markv=True,
            lp=0.1,
            up=0.7,
            upl=0.5,
            distance_aux=True,
            distance_markv=True,
            dis_markv_weight=0.05,
            aux_l1_beta=0.1,
            reg_std=True,
            matching_weights=[1.0, 1.0],
            lines_2=False,
            lines_2_weights=5.0,
            lines_2_mask=[False,False,False,False,False,False,False,False],
            lane_type=True,
            lane_type_weights = 5.0,
            lane_type_mask = [False,True,False,False,False,False,False,False],
            )),
    test_cfg=dict(
        pts=dict(
            point_cloud_range=point_cloud_range,
            post_center_limit_range=[-61.2, -61.2, -10.0, 61.2, 61.2, 10.0],
            max_per_img=500,
            max_pool_nms=False,
            min_radius=[4, 12, 10, 1, 0.85, 0.175],
            score_threshold=0.1,
            out_size_factor=4,
            voxel_size=voxel_size[:2],
            pre_max_size=1000,
            post_max_size=500,

            # Scale-NMS
            nms_type=['rotate'],
            nms_thr=[0.2],
            nms_rescale_factor=[[1.0, 0.7, 0.7, 0.4, 0.55,
                                 1.1, 1.0, 1.0, 1.5, 3.5]]
        ),
        maps=dict(
            point_cloud_range=point_cloud_range,
            grid_size=[512, 512, 40],
            voxel_size=voxel_size,
            out_size_factor=4,
            dense_reg=1,
            max_objs=1e4,
            thr=[0.4,0.4,0.4,0.4,
                 0.2,0.4,0.4,0.2]),
            ))

# Data
dataset_type = 'HJDataset'
data_root = 'data/hj_dataset/'
file_client_args = dict(backend='disk')

# bda_aug_conf = dict(
#     rot_lim=(-0, 0), #(-22.5, 22.5),
#     scale_lim=(1., 1.), #(0.95, 1.05),
#     flip_dx_ratio=0.,
#     flip_dy_ratio=0.)
bda_aug_conf = dict(
    rot_lim=(-22.5, 22.5), #(-22.5, 22.5),
    scale_lim=(0.95, 1.05), #(0.95, 1.05),
    flip_dx_ratio=0.5,
    flip_dy_ratio=0.5)

train_pipeline = [
    dict(
        type='PrepareImageInputsHJ',
        is_train=True,
        data_config=data_config,
        ego_cam=None),
    dict(
        type='LoadAnnotationsBEVDepth',
        bda_aug_conf=bda_aug_conf,
        classes=class_names),
    dict(type='LoadGlobalMatrix'),
    dict(
        type='LoadPointsFromFile',
        coord_type='LIDAR',
        load_dim=5,
        use_dim=4,
        file_client_args=file_client_args),
    # dict(type='PointToMultiViewDepth', downsample=1, grid_config=grid_config),
    dict(type='ObjectRangeFilter', point_cloud_range=point_cloud_range),
    dict(type='ObjectNameFilter', classes=class_names),
    dict(type='DefaultFormatBundle3D', class_names=class_names),
    dict(
        type='Collect3D', 
        keys=['img_inputs', 'points', 'gt_maps_3d', 'gt_bboxes_3d', 'gt_labels_3d','timestamp']),#,'gt_depth'
]

test_pipeline = [
    dict(type='PrepareImageInputsHJ', 
    data_config=data_config,
    ego_cam=None),
    dict(
        type='LoadAnnotationsBEVDepth',
        bda_aug_conf=bda_aug_conf,
        classes=class_names,
        is_train=False),
    dict(type='LoadGlobalMatrix'),
    dict(
        type='LoadPointsFromFile',
        coord_type='LIDAR',
        load_dim=5,
        use_dim=4,
        file_client_args=file_client_args),
    dict(
        type='MultiScaleFlipAug3D',
        img_scale=(1333, 800),
        pts_scale_ratio=1,
        flip=False,
        transforms=[
            dict(
                type='DefaultFormatBundle3D',
                class_names=class_names,
                with_label=False),
            dict(type='Collect3D', keys=['points', 'img_inputs','timestamp'])#,'timestamp','gt_bboxes_3d', 'gt_labels_3d'])
        ])
]
evaluation = dict(interval=160, pipeline=test_pipeline)
input_modality = dict(
    use_lidar=False,
    use_camera=True,
    use_radar=False,
    use_map=False,
    use_external=False)

share_data_config = dict(
    type=dataset_type,
    classes=class_names,
    modality=input_modality,
    img_info_prototype='bevdet',
)

test_data_config = dict(
    pipeline=test_pipeline,
    data_root = data_root,
    ann_file=data_root + 'hjdataset_release_val_v1.0.pkl',# 'highway_val_2943.pkl',# 'hj_infos_train_v1.0.pkl',
    load_interval=1,
    sequ_num=sequ_num,
    sequ_mode = sequ_mode,)  # set datasets interval

data = dict(
    samples_per_gpu=8,
    workers_per_gpu=2,
    train=dict(
        data_root=data_root,
        ann_file=data_root + 'hjdataset_release_train_v1.8.pkl', #'object_test_data.pkl',#
        pipeline=train_pipeline,
        classes=class_names,
        test_mode=False,
        use_valid_flag=True,
        # we use box_type_3d='LiDAR' in kitti and nuscenes dataset
        # and box_type_3d='Depth' in sunrgbd and scannet dataset.
        box_type_3d='LiDAR',
        sequ_num=sequ_num,
        sequ_arfa = 24,
        sequ_mode = False,
        aug_param=dict(
                type='Bev_augmetation',
                bda_aug_conf=bda_aug_conf,
                classes=class_names),
        load_interval=1,
        ),
    val=test_data_config,
    test=test_data_config)

for key in ['train', 'val', 'test']:
    data[key].update(share_data_config)

# Optimizer
optimizer = dict(type='AdamW', lr=2e-4, weight_decay=0.02)
optimizer_config = dict(grad_clip=dict(max_norm=5, norm_type=2))
lr_config = dict(
    policy='step',
    warmup='linear',
    warmup_iters=200,
    warmup_ratio=0.001,
    step=[500])
runner = dict(type='EpochBasedRunner', max_epochs=500)

custom_hooks = [
    dict(
        type='MEGVIIEMAHook',
        init_updates=10560,
        priority='NORMAL',
    ),
    dict(
        type='SequentialControlHook',
        temporal_start_epoch=-1,
    ),
]
# find_unused_parameters = True
# unstable
# fp16 = dict(loss_scale='dynamic')
